#include <stdio.h>
#include <tchar.h>
#include <conio.h>
#include <winsock2.h>
#include <vector>
#include <string>

#include "NatNetTypes.h"
#include "NatNetClient.h"
#include "vector3d.h"

using namespace std;


void __cdecl DataHandler(sFrameOfMocapData* data, void* pUserData);		// receives data from the server
void resetClient();
int CreateClient(int iConnectionType);



NatNetClient* theClient;


int _tmain(int argc, _TCHAR* argv[])
{
	int iResult;

	// Create NatNet Client
	if (CreateClient(ConnectionType_Multicast) != ErrorCode_OK){
		printf("Error initializing client.  See log for details.  Exiting");
		return 1;
	}

	// Ready to receive marker stream!
	int c;
	bool bExit = false;
	while (c = _getch())
	{
		switch (c)
		{
		case 'q':
			bExit = true;
			break;
		default:
			break;
		}
		if (bExit)
			break;
	}

	theClient->Uninitialize();

	return ErrorCode_OK;
}

// Establish a NatNet Client connection
int CreateClient(int iConnectionType)
{
	// release previous server
	if (theClient)
	{
		theClient->Uninitialize();
		delete theClient;
	}

	// create NatNet client
	theClient = new NatNetClient(iConnectionType);


	// Set callback handlers
	theClient->SetDataCallback(DataHandler, theClient);	// this function will receive data from the server

	// Init Client and connect to NatNet server
	// to use NatNet default port assigments
	int retCode = theClient->Initialize("127.0.0.1", "127.0.0.1");
	// to use a different port for commands and/or data:
	//int retCode = theClient->Initialize(szMyIPAddress, szServerIPAddress, MyServersCommandPort, MyServersDataPort);
	if (retCode != ErrorCode_OK)
	{
		printf("Unable to connect to server.  Error code: %d. Exiting", retCode);
		return ErrorCode_Internal;
	}
	else
	{
		// print server info
		sServerDescription ServerDescription;
		memset(&ServerDescription, 0, sizeof(ServerDescription));
		theClient->GetServerDescription(&ServerDescription);
		if (!ServerDescription.HostPresent)
		{
			printf("Unable to connect to server. Host not present. Exiting.");
			return 1;
		}
	}

	return ErrorCode_OK;

}

double calculateCircleRadius(v3d p1, v3d p2, v3d p3){
	v3d t = p2 - p1;
	v3d u = p3 - p1;
	v3d v = p3 - p2;

	v3d w = t / u; // / ist das Kreuzprodukt
	double wsl = w.getlength2();

	// Formel von Wikipedia
	double radius = (t.getlength() * v.getlength() * u.getlength()) /
		(2 * (t / v).getlength());

	return radius;
}
v3d calculateCircleCenter(v3d p1, v3d p2, v3d p3){
	v3d t = p2 - p1;
	v3d u = p3 - p1;
	v3d v = p3 - p2;

	// Helfer
	double nenner = 2.0 * ((p1 - p2) / (p2 - p3)).getlength2();


	double a = v.getlength2() * (p1 - p2) * (p1 - p3) / nenner;
	double b = u.getlength2() * (p2 - p1) * (p2 - p3) / nenner;
	double g = t.getlength2() * (p3 - p1) * (p3 - p2) / nenner;

	return a * p1 + b * p2 + g * p3;
}

// DataHandler receives data from the server
void __cdecl DataHandler(sFrameOfMocapData* data, void* pUserData)
{
	NatNetClient* pClient = (NatNetClient*)pUserData;
	pClient->SetVerbosityLevel(0);

	int i = 0;

	vector<v3d> finger;
	for (i = 0; i < data->nOtherMarkers; i++){
		double x = data->OtherMarkers[i][0];
		double y = data->OtherMarkers[i][1];
		double z = data->OtherMarkers[i][2];


		// Return if hand palm rigid body was not detected
		if (data->nRigidBodies == 0) return;

		// Filter all points and only return the points which are not related to any rigid body
		bool a = true;
		for (int j = 0; j < data->RigidBodies[0].nMarkers; j++){
			double _x = data->RigidBodies[0].Markers[j][0];
			double _y = data->RigidBodies[0].Markers[j][1];
			double _z = data->RigidBodies[0].Markers[j][2];

			double diffX = abs(x - _x);
			double diffY = abs(y - _y);
			double diffZ = abs(z - _z);

			if (diffX < 0.001 && diffY < 0.001 && diffZ < 0.001)
				a = false;
		}

		if (a)	finger.push_back(v3d(x, y, z));
	}

	for (i = 0; i < data->nRigidBodies; i++)
	{
		bool bTrackingValid = data->RigidBodies[i].params & 0x01;

		std::vector<v3d> marker;
		for (int j = 0; j < data->RigidBodies[i].nMarkers; j++)
		{
			float x = data->RigidBodies[i].Markers[j][0];
			float y = data->RigidBodies[i].Markers[j][1];
			float z = data->RigidBodies[i].Markers[j][2];

			marker.push_back(v3d(x, y, z));
		}
	}

	if (finger.size() > 2)
		printf("%f\n", calculateCircleRadius(finger[0], finger[1], finger[2]) * 100 * 10);
}



